#include "OpenCVTrackingThread.hxx"
#include "FramePool.hxx"
#include "Sequence.hxx"
#include "VideoThread.hxx"
#include "TrackedObject.hxx"
#include "Project.hxx"
#include "VideoView.hxx"

OpenCVTrackingThread::OpenCVTrackingThread()
{
	strcpy(_trackerType, "CSRT");

	_isTrackerInitialized = false;
}

OpenCVTrackingThread::~OpenCVTrackingThread()
{
	_tracker.release();
}

void OpenCVTrackingThread::run()
{
	if (!isInitialized())
		initialize();

	while (1)
	{
		if (getStatus() == TR_STATUS_STOPPED) break;

		if (getStatus() == TR_STATUS_STOPPING)
		{
			setStatus(TR_STATUS_STOPPED);
			break;
		}

		FramePool* inputPool = getInputPool();
		FramePool* outputPool = getOutputPool();
		Sequence* model = getSequence();
		char* frameBuffer = getFrameBuffer();

		if (inputPool->popFrame(frameBuffer))
		{
			int frameIndex = *(int*)frameBuffer;

			static int lastFrameIndex = frameIndex;

			if ((frameIndex < lastFrameIndex) || (frameIndex - lastFrameIndex != 1))
			{
				printf("Error!!!\n");
			}
			lastFrameIndex = frameIndex;

			if (getStatus() == TR_STATUS_TRACKING)
			{
				cv::Mat img(model->getVideoHeight(), model->getVideoWidth(), CV_8UC3, frameBuffer + sizeof(VideoFrameInfo));

				static bool trackerInited = false;
				cv::Rect2d bbox;
				std::vector<TrackedObject*>* objects = model->getTrackingObjects();
				if (!_isTrackerInitialized)
				{
					_tracker = cv::MultiTracker::create();
					for (int i = 0; i < objects->size(); i++)
					{
						TrackedObject* tobj = objects->at(i);

						// add template frame to tracked object
						tobj->setStartFrameIndex(frameIndex);
						tobj->setEndFrameIndex(frameIndex);

						//tobj->pushRect(frameIndex, tobj->getTemplateBox());
						Shape* shape = 0;
						ProjectType pt = model->getVideoView()->getProject()->getProjectType();

						if (pt == TYPE_RECTANGLE)
							shape = new ShapeRect(tobj->getTemplateBox());
						else if (pt == TYPE_FISH)
							shape = new ShapeFish(tobj->getTemplateBox());

						tobj->pushShape(frameIndex, shape);


						if (strcmp(_trackerType, "CSRT") == 0)
							_tracker->add(cv::TrackerCSRT::create(), img, tobj->getTemplateBox());
						else if (strcmp(_trackerType, "MOSSE") == 0)
							_tracker->add(cv::TrackerMOSSE::create(), img, tobj->getTemplateBox());
						else if (strcmp(_trackerType, "GOTURN") == 0)
							_tracker->add(cv::TrackerGOTURN::create(), img, tobj->getTemplateBox());
						else if (strcmp(_trackerType, "KCF") == 0)
							_tracker->add(cv::TrackerKCF::create(), img, tobj->getTemplateBox());
						else if (strcmp(_trackerType, "TLD") == 0)
							_tracker->add(cv::TrackerTLD::create(), img, tobj->getTemplateBox());
						else if (strcmp(_trackerType, "MedianFlow") == 0)
							_tracker->add(cv::TrackerMedianFlow::create(), img, tobj->getTemplateBox());
						else if (strcmp(_trackerType, "Boosting") == 0)
							_tracker->add(cv::TrackerBoosting::create(), img, tobj->getTemplateBox());
						else if (strcmp(_trackerType, "MIL") == 0)
							_tracker->add(cv::TrackerMIL::create(), img, tobj->getTemplateBox());
					}
					_isTrackerInitialized = true;
				}
				else
				{
					_tracker->update(img);

				}

				for (unsigned i = 0; i < _tracker->getObjects().size(); i++)
				{

					TrackedObject* tobj = objects->at(i);
					cv::Rect2d rect = _tracker->getObjects()[i];
					tobj->setEndFrameIndex(frameIndex);

					Shape* shape = 0;
					ProjectType pt = model->getVideoView()->getProject()->getProjectType();

					if (pt == TYPE_RECTANGLE)
						shape = new ShapeRect(rect);
					else if (pt == TYPE_FISH)
						shape = new ShapeFish(rect);

					tobj->pushShape(frameIndex, shape);

					//tobj->pushRect(frameIndex, rect);
					//cv::rectangle(img, , cv::Scalar(255, 255, 0), 3);
				}
			}

			//int fullCheckTimes = 0;
			//while (_outputPool->isFull() && fullCheckTimes < 40)
			//{
			//	fullCheckTimes++;
			//	msleep(50);
			//}
			while (outputPool->isFull())
			{
				msleep(50);
			}

			if (!outputPool->pushFrame(frameBuffer))
			{
				msleep(20);
			}
		}
		else
		{
			msleep(20);
		}
	}
	setStatus(TR_STATUS_STOPPED);
}